

#include "pathlif.h"



const double R_en = 6371008.77146855;

double deg2rad(double degrees) {
    return degrees * (PI / 180.0);
}


/**
 ******************************************************************************
 * @function       : 中值滤波:过滤杂波
 * @author         : yall
 * @param          : 
 * @date           : 2025/6/16
 ******************************************************************************
 */
void Median_Fit(double *input, double *output, int n, int window_size) {
    int half_window = window_size / 2;
    
    
    for (int i = 0; i < n; i++) {
        int start = (i - half_window) > 0 ? (i - half_window) : 0;
        int end = (i + half_window) < n ? (i + half_window) : n - 1;
        // 取数据
        double window[MAX_POINTS];
        int window_count = 0;
        for (int j = start; j <= end; j++) {
            window[window_count++] = input[j];
        }
        // 排数据
        for (int j = 0; j < window_count - 1; j++) {
            for (int k = j + 1; k < window_count; k++) {
                if (window[j] > window[k]) {
                    double temp = window[j];
                    window[j] = window[k];
                    window[k] = temp;
                }
            }
        }
        // 输出
        output[i] = window[window_count / 2];
    }
}

/**
 ******************************************************************************
 * @function       : 样条插值函数（简化）
 * @author         : yall
 * @param          : 
 * @date           : 2025/6/16
 ******************************************************************************
 */
void spline_interpolation(double *x, double *y, int n, double *t, double *result, int m) {
    for (int i = 0; i < m; i++) {
        double t_val = t[i];
        int idx = 0;
        while (idx < n - 1 && x[idx + 1] < t_val) {
            idx++;
        }
        double x0 = x[idx];
        double x1 = x[idx + 1];
        double y0 = y[idx];
        double y1 = y[idx + 1];
        result[i] = y0 + (y1 - y0) * (t_val - x0) / (x1 - x0);
    }
}

/**
 ******************************************************************************
 * @function       : 输出格式
 * @author         : yall
 * @param          : 
 * @date           : 2025/6/16
 ******************************************************************************
 */
struct ArrayWrapper {
    double data_xy[max_points][2];
};

/**
 ******************************************************************************
 * @function       : 最终处理函数
 * @author         : yall
 * @param          : in:一行两列数组；out:[struct ArrayWrapper] xy
 * @date           : 2025/6/16
 ******************************************************************************
 */
void ArrayWrapper_Path_fit(double data[MAX_POINTS][2])
 {
    int n = MAX_POINTS; // 实际数据点数
    

    // 删除 {0, 0} 的数据
    double new_data[MAX_POINTS][2];
    int new_count = 0;
    for (int i = 0; i < n; i++) {
        if (data[i][0] != 0 || data[i][1] != 0) {
            new_data[new_count][0] = data[i][0];
            new_data[new_count][1] = data[i][1];
            new_count++;
        }
    }

    // 提取经纬度数据
    double longitude[MAX_POINTS], latitude[MAX_POINTS];
    for (int i = 0; i < new_count; i++) {
        longitude[i] = new_data[i][0];
        latitude[i] = new_data[i][1];
    }

    // 计算与第一个点的经纬度差值
    double delta_lon[MAX_POINTS], delta_lat[MAX_POINTS];
    for (int i = 0; i < new_count; i++) {
        delta_lon[i] = longitude[i] - longitude[0];
        delta_lat[i] = latitude[i] - latitude[0];
    }

    // 转换为米单位坐标
    double delta_x[MAX_POINTS], delta_y[MAX_POINTS];
    for (int i = 0; i < new_count; i++) {
        delta_x[i] = delta_lon[i] * (PI / 180.0) * R_en * cos(deg2rad(latitude[i]));
        delta_y[i] = delta_lat[i] * (PI / 180.0) * R_en;
    }

    // 中值滤波
    int window_size = 7;
    double x_filtered[MAX_POINTS], y_filtered[MAX_POINTS];
    Median_Fit(delta_x, x_filtered, new_count, window_size);
    Median_Fit(delta_y, y_filtered, new_count, window_size);

    // 删除离群值
    double residual_x[MAX_POINTS], residual_y[MAX_POINTS];
    for (int i = 0; i < new_count; i++) {
        residual_x[i] = delta_x[i] - x_filtered[i];
        residual_y[i] = delta_y[i] - y_filtered[i];
    }
    double threshold_x = 0.1, threshold_y = 0.1; // 阈值
    int outliers[MAX_POINTS] = {0};
    for (int i = 0; i < new_count; i++) {
        if (fabs(residual_x[i]) > threshold_x || fabs(residual_y[i]) > threshold_y) {
            outliers[i] = 1;
        }
    }
    double x_cleaned[MAX_POINTS], y_cleaned[MAX_POINTS];
    int cleaned_count = 0;
    for (int i = 0; i < new_count; i++) {
        if (!outliers[i]) {
            x_cleaned[cleaned_count] = x_filtered[i];
            y_cleaned[cleaned_count] = y_filtered[i];
            cleaned_count++;
        }
    }

    // 参数化数据点：计算累积距离
    double dist[MAX_POINTS] = {0};
    for (int i = 1; i < cleaned_count; i++) {
        dist[i] = sqrt(pow(x_cleaned[i] - x_cleaned[i - 1], 2) + pow(y_cleaned[i] - y_cleaned[i - 1], 2));
    }
    double cumdist[MAX_POINTS] = {0};
    for (int i = 1; i < cleaned_count; i++) {
        cumdist[i] = cumdist[i - 1] + dist[i];
    }

    // 生成均匀分布的参数值
    int num_points = 10000;
    double t_uniform[num_points];
    for (int i = 0; i < num_points; i++) {
        t_uniform[i] = (cumdist[cleaned_count - 1] * i) / (num_points - 1);
    }

    // 样条插值拟合
    double x_fit[num_points], y_fit[num_points];
    spline_interpolation(cumdist, x_cleaned, cleaned_count, t_uniform, x_fit, num_points);
    spline_interpolation(cumdist, y_cleaned, cleaned_count, t_uniform, y_fit, num_points);

    // 均匀采样30个点
    int num_uniform = 30;
    double t_uniform_samples[num_uniform];
    for (int i = 0; i < num_uniform; i++) {
        t_uniform_samples[i] = (cumdist[cleaned_count - 1] * i) / (num_uniform - 1);
    }
    double x_uniform[num_uniform], y_uniform[num_uniform];
    spline_interpolation(cumdist, x_cleaned, cleaned_count, t_uniform_samples, x_uniform, num_uniform);
    spline_interpolation(cumdist, y_cleaned, cleaned_count, t_uniform_samples, y_uniform, num_uniform);

    //struct ArrayWrapper xy;
    GeoLocation_t  GPS_Data;
    // 输出均匀采样的点
    for (int i = 0; i < num_uniform; i++) {
        uint32_t temp[2]={0};
        
        GPS_Data.latitude=0;
        GPS_Data.longitude=0;
        GPS_Data.longitude = x_uniform[i];
        GPS_Data.latitude = y_uniform[i];
            temp[0]=GPS_Data.latitude*Ten_seven;
            temp[1]=GPS_Data.longitude*Ten_seven;
            LOGY(" RTK_head lon:%d,lat:%d,%d,%d\n",temp[1],temp[0],num_uniform,i);
            HAL_Delay(2);
        BaseType_t pd = LinkedList_Append(RTK_head,&GPS_Data); // 数据存链表 写
        if(pd!=pdPASS) LOG(" Write Link Failing.DP....\n");
        
    }

    //return xy;
}
 
//void Median_Fit_c(studio_line_c *line, studio_line_c *vacuate_line, int n, int window_size) {
//    int half_window = window_size / 2;
//    studio_point_c l_data;
//    studio_point_c k_data;
//    //studio_line_c vacuate_line = studio_line_c_init(); 
//    for (int i = 0; i < n; i++) {
//        int start = (i - half_window) > 0 ? (i - half_window) : 0;
//        int end = (i + half_window) < n ? (i + half_window) : n - 1;
//        // 取数据
//        //double window[MAX_POINTS];
//        int window_count = 0;
//        studio_line_c Temp_line = studio_line_c_init(); 
//        for (int j = start; j <= end; j++) {
//            memset(&l_data, 0, sizeof(l_data));
//            l_data = studio_line_c_get_point(line,j);
//            studio_point_c point = studio_point_init(l_data.x, l_data.y);
//            studio_line_c_add_point(&Temp_line, point);
//        }
//        window_count = Temp_line.size;
//        // 排数据
//        for (int l = 0; l < window_count - 1; l++) {
//            for (int k = l + 1; k < window_count; k++) {
//                memset(&l_data, 0, sizeof(l_data));
//                memset(&l_data, 0, sizeof(k_data));
//                l_data = studio_line_c_get_point(&Temp_line,l);
//                k_data = studio_line_c_get_point(&Temp_line,k);
//                if (l_data.x > k_data.x) {
//                    double temp = l_data.x;
//                    l_data.x = k_data.x;
//                    k_data.x = temp;
//                }else if (l_data.y > k_data.y)
//                {
//                    double temp = l_data.y;
//                    l_data.y = k_data.y;
//                    k_data.y = temp;
//                }
//                studio_line_c_set_point(&Temp_line, l, l_data);
//                studio_line_c_set_point(&Temp_line, k, k_data);
//            }
//        }
//        memset(&l_data, 0, sizeof(l_data));
//        l_data = studio_line_c_get_point(&Temp_line,window_count / 2);
//        studio_point_c point = studio_point_init(l_data.x, l_data.y);
//        studio_line_c_add_point(vacuate_line, point);
//        studio_line_c_destroy(&Temp_line);
//    }
//    studio_line_c_destroy(line);
//}

//void ArrayWrapper_Path_Fit(studio_line_c *line,const uint16_t size,studio_line_c *vacuate_line)
// {
//    uint16_t num = line->size; // 实际数据点数
//    double lon_0,lat_0=0; // 首点经纬度
//     int window_size = size; //中值
//     studio_point_c stu_data;

//    if (line->size == 0)
//    {
//        LOG("line is empty\n");
//        return ;
//    }
//    
//    stu_data = studio_line_c_get_point(line,0);
//    lon_0 = stu_data.x;
//    lat_0 = stu_data.y;

//    studio_line_c mi_line = studio_line_c_init(); 
//    for (int i = 0; i < line->size; i++)
//    {
//        memset(&stu_data, 0, sizeof(stu_data));
//        studio_point_c temp_data = studio_line_c_get_point(line,i);
//        stu_data.x = (stu_data.x - lon_0) * (PI / 180.0) * R_en * cos(deg2rad(temp_data.y));
//        stu_data.y = (stu_data.y - lat_0) * (PI / 180.0) * R_en;
//        studio_point_c point = studio_point_init(stu_data.x, stu_data.y);
//        studio_line_c_add_point(&mi_line, point);
//        // 转换为米单位坐标
//    }
//    studio_line_c_destroy(line);
//    
//    // 中值滤波
//    studio_line_c fil_line = studio_line_c_init();
//    Median_Fit_c(&mi_line, &fil_line, mi_line.size, window_size);
//    studio_line_c_destroy(&mi_line);

//    // 删除离群值
//    double residual_x[MAX_POINTS], residual_y[MAX_POINTS];
//    for (int i = 0; i < new_count; i++) {
//        residual_x[i] = delta_x[i] - x_filtered[i];
//        residual_y[i] = delta_y[i] - y_filtered[i];
//    }
//    double threshold_x = 0.1, threshold_y = 0.1; // 阈值
//    int outliers[MAX_POINTS] = {0};
//    for (int i = 0; i < new_count; i++) {
//        if (fabs(residual_x[i]) > threshold_x || fabs(residual_y[i]) > threshold_y) {
//            outliers[i] = 1;
//        }
//    }
//    double x_cleaned[MAX_POINTS], y_cleaned[MAX_POINTS];
//    int cleaned_count = 0;
//    for (int i = 0; i < new_count; i++) {
//        if (!outliers[i]) {
//            x_cleaned[cleaned_count] = x_filtered[i];
//            y_cleaned[cleaned_count] = y_filtered[i];
//            cleaned_count++;
//        }
//    }

//    // 参数化数据点：计算累积距离
//    double dist[MAX_POINTS] = {0};
//    for (int i = 1; i < cleaned_count; i++) {
//        dist[i] = sqrt(pow(x_cleaned[i] - x_cleaned[i - 1], 2) + pow(y_cleaned[i] - y_cleaned[i - 1], 2));
//    }
//    double cumdist[MAX_POINTS] = {0};
//    for (int i = 1; i < cleaned_count; i++) {
//        cumdist[i] = cumdist[i - 1] + dist[i];
//    }

//    // 生成均匀分布的参数值
//    int num_points = 10000;
//    double t_uniform[num_points];
//    for (int i = 0; i < num_points; i++) {
//        t_uniform[i] = (cumdist[cleaned_count - 1] * i) / (num_points - 1);
//    }

//    // 样条插值拟合
//    double x_fit[num_points], y_fit[num_points];
//    spline_interpolation(cumdist, x_cleaned, cleaned_count, t_uniform, x_fit, num_points);
//    spline_interpolation(cumdist, y_cleaned, cleaned_count, t_uniform, y_fit, num_points);

//    // 均匀采样30个点
//    int num_uniform = 30;
//    double t_uniform_samples[num_uniform];
//    for (int i = 0; i < num_uniform; i++) {
//        t_uniform_samples[i] = (cumdist[cleaned_count - 1] * i) / (num_uniform - 1);
//    }
//    double x_uniform[num_uniform], y_uniform[num_uniform];
//    spline_interpolation(cumdist, x_cleaned, cleaned_count, t_uniform_samples, x_uniform, num_uniform);
//    spline_interpolation(cumdist, y_cleaned, cleaned_count, t_uniform_samples, y_uniform, num_uniform);

//    //struct ArrayWrapper xy;
//    GeoLocation_t  GPS_Data;
//    // 输出均匀采样的点
//    for (int i = 0; i < num_uniform; i++) {
//        uint32_t temp[2]={0};
//        
//        GPS_Data.latitude=0;
//        GPS_Data.longitude=0;
//        GPS_Data.longitude = x_uniform[i];
//        GPS_Data.latitude = y_uniform[i];
//            temp[0]=GPS_Data.latitude*Ten_seven;
//            temp[1]=GPS_Data.longitude*Ten_seven;
//            LOGY(" RTK_head lon:%d,lat:%d,%d,%d\n",temp[1],temp[0],num_uniform,i);
//            HAL_Delay(2);
//        BaseType_t pd = LinkedList_Append(RTK_head,&GPS_Data); // 数据存链表 写
//        if(pd!=pdPASS) LOG(" Write Link Failing.DP....\n");
//        
//    }

//    //return xy;
//}
 









